/*************************************************************************************************\
*																																																	*
* PROGRAM: Gyro       	      																																		*
* VERSION: 0																																											*
* PURPOSE: This program provides basic gyro functions like calculating offset and returning     	*
*					 readings that are corrected for offset and battery power		                            *
* AUTHOR:	 Aswin Bouwmeester (aswin.bouwmeester@gmail.com)																				*
* DATE:		 okt 10, 2010	  																																				*
*																																																	*
\*************************************************************************************************/

#pragma systemfile
#pragma autoStartTasks

#ifndef _SENSORS_
  #include "sensors.c";
#endif

#ifndef toRad
  #define torad 0.017453
#endif

typedef struct{
  float intersect;
  float slope;
  float voltage;
  float turnrate;
  float cum_turn;
  float heading;
  int samplesize;
} tGyro;

tGyro Pgyro;

void initGyro(tSensors port, int samplesize);
float readGyro(tSensors port);
void updateGyroOffset(tSensors port);
void calculateGyroOffset(tSensors port);
float getGyroOffset();


void initGyro(tSensors port, int samplesize)
{
  Pgyro.slope=0.004981;
  Pgyro.samplesize=samplesize;
  calculateGyroOffset( port);
}

void calculateGyroOffset(tSensors port)
{
  float offset;
  float voltage;
  float i;
  for (i=1;i<=Pgyro.samplesize;i++)
  {
    Pgyro.intersect=(i-1.0)/i*Pgyro.intersect + ((float)SensorValue(port)-Pgyro.slope*(float)nImmediateBatteryLevel)/i;
		wait1Msec(4);
  }
}

float getGyroOffset()
{
  return Pgyro.slope * (float)nImmediateBatteryLevel + Pgyro.intersect;
}

float readGyro(tSensors port)
{
  float val;
  val=(float)SensorValue(port)-getGyroOffset();
  return val;
}

void updateGyroOffset(tSensors port)
{
  Pgyro.intersect=(Pgyro.samplesize-1.0)/Pgyro.samplesize*Pgyro.intersect + ((float)SensorValue(port)-Pgyro.slope*(float)nImmediateBatteryLevel)/Pgyro.samplesize;
}

task gyroMean()
{
  int i;
  tSensors _gyro;
  float gyro_measured;
  _gyro=findSensor(sensorI2CHiTechnicGyro);
  initGyro(_gyro,500);
  while (true)
  {
		gyro_measured=0;
		for (i=0;i<5;i++)
		{
		  wait1Msec(4);
		  gyro_measured+=readGyro(_gyro);
		}
		Pgyro.turnrate=- gyro_measured/5.0;
		filteredSensorValue[_gyro]=Pgyro.turnrate;
		if (Pgyro.turnrate<.5)
		{Pgyro.turnrate=0;}
		Pgyro.cum_turn=Pgyro.cum_turn +Pgyro.turnrate*20.0/1000.0;
		Pgyro.heading=Pgyro.cum_turn;
		if (Pgyro.heading>=360 )
		  Pgyro.heading-= 360;
		if (Pgyro.heading<0)
		  Pgyro.heading += 360;
  }
}
